Skip to content


ai  101  pytorch  classification  nvidia  cuda  install  tensorrt  yolo  ardupilot  None  ros2  dds  micro ros  xrce  sitl  plugin  SITL  debug  rangefinder  pymavlink  mavros  gazebo  distance sensor  system_time  timesync  cmake  gtest  ctest  cpp  c++  format  fmt  multithreading  spdlog  camera  coordinate system  orb  matching  opencv  build  transformation  computer vision  homography  optical flow  of  trackers  cv  cyclonedds  eprosima  fastdds  simulation  config  ignition  bridge  sdf  tips  ign-transport  sensors  lidar  aptly  apt  encryption  pgp  docker  git  bundle  github  hooks  pre-commit  lxd  container  lxc  x11  profile  vscode  marpit  presentation  marp  markdown  mermaid  video  ffmpeg  gstreamer  cheat-sheet  sdp  v4l2loopback  gi  snippets  cheat Sheet  python  asyncio  future  click  cli  numpy  project  template  black  isort  docs  project document  docstrings  flake8  linter  git-hook  mypy  unittest  pytest  pylint  mock  iterator  generator  logging  tuple  namedtuple  typing  annotation  pyzmq  zmq  msgpack  action  namespace  remap  control2  ros2_control  gdb  qos  tag  plugins  msg  node  zero-copy  shm  tutorial  algorithm  calibration  diff  pid  dev  colcon  colcon_cd  rpi  arm  qemu  settings  behavior  plot  visualization  debugging  diagnostic  diagnostics  tutorials  gst  math  apm  rat_runtime_monitor  web  rosbridge  vue  binding  discovery  gazebo-classic  launch  spawn  cook  gps  imu  ray  gazebo_ros_ray_sensor  ultrsonic  range  ultrasonic  gazebo classic  wrench  effort  odom  ign  gz  xacro  ros_ign  diff_drive  odometry  joint_state  argument  OpaqueFunction  DeclareLaunchArgument  LaunchConfiguration  tmux  nav  slam  test  rclpy  executor  MultiThreadedExecutor  SingleThreadedExecutor  param  dynamic-reconfigure  service  client  setup.py  package.xml  parameter  parameters  custom  msgs  executers  pub  sub  rqt  rviz  rviz2  pose  marker  tf2  deb  package  setup  local_setup  rosdep  package manager  project settings  vcstool  cross-compiler  nano  texture  tmuxp  rootfs  embedded  zah  linux  rm  ubuntu  ip  ss  network  netstat  snap  deploy  ssh  systemd  mkdocs  extensions  socat  networking  serial  udp  tc  mtu  select  px4  robotics  kalman_filter  kalman  filter  control  todo  vscode-ext  json  yaml  schema  yocto  poky  world  gazebo_ros2_control  position_controller  effort_controller  velocity_controller  urdf  gazebo_ros_force  gazebo_ros_joint_state_publisher  robot_state_publisher  joint_state_publisher  projects  vrx  buoyancy 

ROS2 module plugin template


Module plugin minimal template show how to

  • subscribe message from ROS
  • publish message to ROS
  • logging

The minimal project include two projects

  • Plugin (demo_gazebo_plugin)
  • Tester (demo_gazebo_plugin_sim)

Projects#

# Plugin
demo_gazebo_plugin
├── CMakeLists.txt
├── include
│   └── demo_gazebo_plugin
│       └── demo_gazebo_plugin.hpp
├── package.xml
└── src
    └── demo_gazebo_plugin.cpp

# Tester
demo_gazebo_plugin_sim/
├── CMakeLists.txt
├── include
│   └── demo_gazebo_plugin_sim
├── launch
│   └── sim_bringup.launch.py
├── models
│   └── demo_model
│       ├── model.config
│       └── model.sdf
├── package.xml
└── src

Plugin#

ros2 pkg create demo_gazebo_plugin \
--build-type ament_cmake \
--dependencies rclcpp std_msgs gazebo_ros
demo_gazebo_plugin.hpp
#ifndef DEMO_GAZEBO_PLUGIN_HPP
#define DEMO_GAZEBO_PLUGIN_HPP

#include <gazebo/common/PID.hh>
#include <gazebo/common/Plugin.hh>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/string.hpp"

namespace demo_gazebo_plugin
{
    class DemoGazeboPlugin : public gazebo::ModelPlugin
    {
    public:
        DemoGazeboPlugin();
        void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);

    private:
        rclcpp::Node::SharedPtr ros_node_;
        rclcpp::Subscription<std_msgs::msg::String>::SharedPtr command_sub_;
        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr command_pub_;
        void sub_handler(const std_msgs::msg::String::SharedPtr msg);
    };
}

#endif

plugin#

demo_gazebo_plugin.cpp
#include "demo_gazebo_plugin/demo_gazebo_plugin.hpp"
#include <gazebo_ros/node.hpp>
#include <std_msgs/msg/string.hpp>

using namespace std::placeholders;

namespace demo_gazebo_plugin
{   
    const std::string SUB_TOPIC = "/demo_gazebo_cmd";
    const std::string PUB_TOPIC = "/demo_gazebo_echo";

    DemoGazeboPlugin::DemoGazeboPlugin()
    {

    }

    void DemoGazeboPlugin::Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf){
        ros_node_ = gazebo_ros::Node::Get(sdf);
        RCLCPP_INFO(ros_node_->get_logger(), "info Demo Gazebo Plugin");
        RCLCPP_WARN(ros_node_->get_logger(), "warning Demo Gazebo Plugin");
        RCLCPP_ERROR(ros_node_->get_logger(), "error Demo Gazebo Plugin");
        // gzmsg << "gz message\n";
        // gzerr << "gz error message\n";

        command_pub_ = ros_node_->create_publisher<std_msgs::msg::String>(PUB_TOPIC, 10);
        command_sub_ = ros_node_->create_subscription<std_msgs::msg::String>(
                SUB_TOPIC,
                10,
                std::bind(&DemoGazeboPlugin::sub_handler, this, _1)
            );
    }

    void DemoGazeboPlugin::sub_handler(const std_msgs::msg::String::SharedPtr msg){
        RCLCPP_WARN(ros_node_->get_logger(), "------ %s ------", msg->data.c_str());
        auto echo_msg = std_msgs::msg::String();
        echo_msg.data = msg->data + "_echo";
        command_pub_->publish(echo_msg);
    }

GZ_REGISTER_MODEL_PLUGIN(DemoGazeboPlugin)
}

CmakeLists.txt#

CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(demo_gazebo_plugin)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(gazebo_ros REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

add_library(demo_gazebo_plugin SHARED src/demo_gazebo_plugin.cpp)

target_include_directories(demo_gazebo_plugin PUBLIC
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>)

ament_target_dependencies(demo_gazebo_plugin
  "gazebo_ros"
  "rclcpp"
  "std_msgs")

install(TARGETS
  demo_gazebo_plugin
  DESTINATION share/${PROJECT_NAME})

ament_package()

Test Project#

ros2 pkg create demo_gazebo_plugin \
--build-type ament_cmake \
--dependencies demo_gazebo_plugin
  • Add plugin to model
    <plugin name="demo_gazebo_plugin" filename="libdemo_gazebo_plugin.so"/>
    

launch#

  • Set plugin path
  • Launch gazebo
  • Spawn model
sim_bringup.launch.py
from launch import LaunchDescription
import os
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import (
    AppendEnvironmentVariable, 
    DeclareLaunchArgument)
from launch.substitutions import LaunchConfiguration
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
import xacro


PACKAGE = "demo_gazebo_plugin_sim"
WORLD = "empty.world"
MODEL = "demo_model"

def generate_launch_description():
    ld = LaunchDescription()

    pkg = get_package_share_directory(PACKAGE)
    pkg_plugin = get_package_share_directory("demo_gazebo_plugin")
    gazebo_pkg = get_package_share_directory('gazebo_ros')

    verbose = LaunchConfiguration("verbose")
    arg_gazebo_verbose = DeclareLaunchArgument("verbose", default_value="true")
    world = LaunchConfiguration("world")
    arg_gazebo_world = DeclareLaunchArgument("world", default_value=WORLD)
    sim_time = LaunchConfiguration("sim_time")
    arg_sim_time = DeclareLaunchArgument("sim_time", default_value="true")

    resources = [os.path.join(pkg, "worlds")]

    resource_env = AppendEnvironmentVariable(
        name="GAZEBO_RESOURCE_PATH", value=":".join(resources)
    )

    plugins = [pkg_plugin]

    plugin_env = AppendEnvironmentVariable(
        name="GAZEBO_PLUGIN_PATH", value=":".join(plugins)
    )

    robot_description_path = os.path.join(pkg, "models", MODEL, "model.sdf")
    doc = xacro.parse(open(robot_description_path))
    xacro.process_doc(doc)
    robot_description = doc.toxml()

    robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        parameters=[{"use_sim_time": sim_time, "robot_description": robot_description}],
    )

    gazebo = IncludeLaunchDescription(
                PythonLaunchDescriptionSource([os.path.join(
                    gazebo_pkg, 'launch', 'gazebo.launch.py')]),
                    launch_arguments={'verbose': verbose, "world": world}.items()
             )
    spawn_entity = Node(
        package="gazebo_ros",
        executable="spawn_entity.py",
        arguments=["-entity", "demo", "-topic", "robot_description", "-z", "0.0"],
        output="screen",
    )

    ld.add_action(resource_env)
    ld.add_action(plugin_env)
    ld.add_action(arg_gazebo_verbose)
    ld.add_action(arg_gazebo_world)
    ld.add_action(arg_sim_time)
    ld.add_action(robot_state_publisher)
    ld.add_action(gazebo)
    ld.add_action(spawn_entity)


    return ld

CMakeLists.txt#

CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(demo_gazebo_plugin_sim)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(demo_gazebo_plugin REQUIRED)


install(DIRECTORY 
  launch
  models
  DESTINATION share/${PROJECT_NAME})


ament_package()

Run / Usage#

terminal1
# Run gazebo
ros2 launch demo_gazebo_plugin_sim sim_bringup.launch.py
terminal2
# pub message to plugin
ros2 topic pub --once /demo_gazebo_cmd std_msgs/String "data: hello"
publisher: beginning loop
publishing #1: std_msgs.msg.String(data='hello')
terminal2
# sub message from gazebo
ros2 topic echo /demo_gazebo_echo 
data: hello_echo
---

Reference#